#!/usr/bin/env python
#coding:utf-8
  
import rospy
import message_filters
from std_msgs.msg import String
from darknet_ros_msgs.msg import BoundingBoxes
from sensor_msgs.msg import Image

isEM= 0
isProcessed= 0
# define publisher
pub_em = rospy.Publisher('emergency', String, queue_size=10)
pub_empic = rospy.Publisher('emergency_pic', Image, queue_size=10)
def callback(data, image):
    result= data.bounding_boxes[0].Class
    detected_class="none"
    if result == 'person':
        # if width is bigger than length, we think the person has fallen
        width = data.bounding_boxes[0].xmax - data.bounding_boxes[0].xmin
        length = data.bounding_boxes[0].ymax - data.bounding_boxes[0].ymin
        if width > length:
            print "person falled"
            detected_class='fallen person'  
            global isEM
            isEM= 1
    elif result == 'fire':
        print "fire detected!"
        detected_class='fire'
        global isEM
        isEM= 1
    else:
        global isProcessed
        isProcessed= 0
        
    if isEM==1 and isProcessed==0:
        global isProcessed
        isProcessed= 1
        pub_em.publish(detected_class)
        pub_empic.publish(image)


# define subscriber
sub_dect = message_filters.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes)
sub_pic = message_filters.Subscriber('/darknet_ros/detection_image', Image)


def funct():
    rospy.init_node('detection_controller')    
    sync = message_filters.ApproximateTimeSynchronizer([sub_dect, sub_pic], 10,1) # merge two subscribers into one callback
    sync.registerCallback(callback)
    rospy.spin()
# susbcribe：bouding_boxes, image.
# publish: image(when a fallen person or fire is deteted)
if __name__ == '__main__':
    try:
        funct()
    except rospy.ROSInterruptException:
        pass
    